#include "modbus_slave.h"
#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include <assert.h>

ModbusSlave::ModbusSlave(const std::shared_ptr<rclcpp::Node> node):
    ModbusCommunication(node)
{
    m_init_ok = false;
}

ModbusSlave::ModbusSlave(const std::shared_ptr<rclcpp::Node> node, std::string name):
    ModbusCommunication(node)
{
    m_init_ok = false;
}

ModbusSlave::~ModbusSlave()
{ 
    printf("ModbusSlave Destructor function return: \n");
    m_quit_mutex.unlock();
    m_thread.join();
    for (size_t slave_i = 0; slave_i < m_Slaves.size(); slave_i++)
    {
        for(size_t cmd_i;cmd_i< m_Slaves[slave_i].readParas.size(); cmd_i++){
            free(m_Slaves[slave_i].readParas[cmd_i].pMapping);
            m_Slaves[slave_i].readParas[cmd_i].wCFG_bit.reset();
            m_Slaves[slave_i].readParas[cmd_i].wCFG_reg.reset();
        }
    }
}
void ModbusSlave::copy2mapping(uint8_t *dest, int len, std_msgs::msg::UInt8MultiArray &newData)
{
    if (newData.data.size() == 0){
        RCLCPP_ERROR(rclcpp::get_logger("Modbus"), "[isEqual]newData size is zero!!"); 
        return;
    }

    RCLCPP_INFO_STREAM(rclcpp::get_logger("Modbus"), "copy2mapping len:" << len << ",data size:" << newData.data.size());
    uint8_t src_arr[newData.data.size()]; 
    memcpy(src_arr, &newData.data[0], newData.data.size() * sizeof(newData.data[0]));
    modbus_set_bits_from_bytes(dest, 0, len, src_arr);
}

void ModbusSlave::copy2mapping(uint16_t *dest, int len, std_msgs::msg::UInt16MultiArray &newData)
{
    if (newData.data.size() == 0){
        RCLCPP_ERROR(rclcpp::get_logger("Modbus"), "[isEqual]newData size is zero!!"); 
        return;
    }

    for (size_t i = 0; i < newData.data.size(); i++)
    {
        dest[i]= newData.data[i];
    }
}

void ModbusSlave::start()
{
    if (m_init_ok)
    {
      m_quit_mutex.lock();
      m_thread = std::thread(&ModbusSlave::thread_cycle, this, this);
    }
    else
    {
      RCLCPP_ERROR(rclcpp::get_logger("Modbus"), "[ModbusCommunication] m_init_ok =%d", m_init_ok);  
    }
}

void ModbusSlave::stop()
{
    m_quit_mutex.unlock();
    m_thread.join();
}

int ModbusSlave::init(std::string prefix)
{
    std::lock_guard<std::timed_mutex> locker(m_quit_mutex);

    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "ModbusSlave init func");
    if(m_init_ok){
        return ALREADY_INIT_ERRO;
    }
    threadStatePub_ = nh_->create_publisher<agv_msgs::msg::MBSlaveStates>(prefix + "/state", 10);
    
    //获取链路层参数
    get_linkpara(prefix);

    //MRC该端口作为从站设备，不能在此类中初始化
    if (m_is_slave)    
    {
        RCLCPP_ERROR(rclcpp::get_logger("Modbus"), "the port protocol is_slave is %d,can't be init in ModbusSlave class",m_is_slave);
        return IS_SLAVE_ERRO;
    }
    
    std::string temp;   
    // step4:更新从站前缀
    prefix= prefix + "/protocol/master_para";

    // 周期读rate hz
    temp= prefix + "/pollInterval";
    nh_->get_parameter_or(temp, m_pollInterval, 0.0f);
    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%f", temp.c_str(), m_pollInterval);

    // 周期读rate hz
    temp= prefix + "/threadInterval";
    nh_->get_parameter_or(temp, m_threadInterval, 10);
    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%d", temp.c_str(), m_threadInterval);

   // 命令应答超时时间
    float  resptime;
    temp= prefix + "/resptime";
    nh_->get_parameter_or(temp, resptime, 0.1f);
    m_resptime= resptime * USEC_OF_1S;
    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%u", temp.c_str(), m_resptime);

    // step5:获取从站个数
    int slaveNum= 0;
    // step4:更新从站前缀
    temp= prefix + "/slaveNum";
    nh_->get_parameter_or(temp, slaveNum, 0);
    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s: %d",temp.c_str(), slaveNum);

    // step6:遍历从站参数，创建从站对象
    prefix= prefix + "/slave";              
    for (int slave_i = 1; slave_i <= slaveNum; slave_i++) {
        std::string preTemp= prefix + std::to_string(slave_i);
        RCLCPP_INFO_STREAM(rclcpp::get_logger("Modbus"), "The ModbusMaster parameter ------");
        RCLCPP_INFO_STREAM(rclcpp::get_logger("Modbus"), "slave:" << slave_i);

        Slave_Para slave;

        temp = preTemp +"/device_name";
        nh_->get_parameter_or<std::string>(temp, slave.deviceName, "unknowDevice"+ std::to_string( slave_i));
        RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%s", temp.c_str(), slave.deviceName.c_str());                

        // 周期读参数
        temp= preTemp + "/CmdNum";
        nh_->get_parameter_or(temp, slave.cmdNum, 0);
        RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%d", temp.c_str(), slave.cmdNum);

        preTemp= preTemp + "/Cmd000";
        for (int i = 1; i <= slave.cmdNum; ++i) {
            // Create modbus cmd parameter
            RCLCPP_INFO(rclcpp::get_logger("Modbus"), "-----------------------------");            
            MB_cyclePara cmdPara;
            // memset(&cmdPara, 0, sizeof( MB_cyclePara));

            cmdPara.index = i;
            RCLCPP_INFO(rclcpp::get_logger("Modbus"), "ModbusSlave polling para index:%d",cmdPara.index);
            
            std::string pre= preTemp + std::to_string(cmdPara.index);

            temp = pre+"/tickexpire";
            nh_->get_parameter_or(temp, cmdPara.tickexpire, 0);
            RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%d", temp.c_str(), cmdPara.tickexpire);

            temp = pre +"/slaveID";
            nh_->get_parameter_or(temp, cmdPara.slaveID, 11);
            RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%d", temp.c_str(), cmdPara.slaveID);

            temp = pre +"/function";
            nh_->get_parameter_or(temp, cmdPara.function, MODBUS_FC_READ_HOLDING_REGISTERS);
            RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%d", temp.c_str(), cmdPara.function);

            // 存储数组类型
            int pointType;
            temp = pre +"/arr_type";
            nh_->get_parameter_or<int>(temp, pointType, 2);         
            cmdPara.arr_type = ModbusPointType(pointType);
            RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%d", temp.c_str(), cmdPara.arr_type);

            temp = pre +"/topic_name";
            nh_->get_parameter_or<std::string>(temp, cmdPara.topic_name, DefaultTopicName + std::to_string(cmdPara.index));
            RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%s", temp.c_str(), cmdPara.topic_name.c_str());

            temp = pre +"/topic_name_sub";
            nh_->get_parameter_or<std::string>(temp, cmdPara.topic_name_sub, DefaultTopicSubName + std::to_string(cmdPara.index));
            RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%s", temp.c_str(), cmdPara.topic_name_sub.c_str());

            temp = pre +"/number";
            nh_->get_parameter_or(temp, cmdPara.number, 10); 
            RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%d", temp.c_str(), cmdPara.number);
            
            temp = pre +"/address";
            nh_->get_parameter_or(temp, cmdPara.address, 0);
            RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%d", temp.c_str(), cmdPara.address);

            std::string name = slave.deviceName + "/" + cmdPara.topic_name;
            switch(cmdPara.arr_type) { 
                case COIL_STATUS:    //  0x：可读可写的PLC线圈个数    
                case INPUT_STATUS:   //1x： 只读PLC输入点     
                    {
                        cmdPara.pub = nh_->create_publisher<std_msgs::msg::UInt8MultiArray>(name, 1);        
                        uint8_t * tab_bit =(uint8_t *) malloc(cmdPara.number * sizeof(uint8_t));
                        if (tab_bit == NULL) {
                            free(tab_bit);
                            return MALLOC_BIT_NULL;
                        }
                        memset(tab_bit, 0, cmdPara.number * sizeof(uint8_t));
                        cmdPara.pMapping= tab_bit;   
                    }
                    break;
                case HOLDING_REGISTER:     //4x： 可读可写的 PLC数据寄存器        
                case INPUT_REGISTER:     //3x：  只读的PLC寄存器
                    {
                        cmdPara.pub = nh_->create_publisher<std_msgs::msg::UInt16MultiArray>(name, 1); 
                        uint16_t * tab_reg =(uint16_t *) malloc(cmdPara.number * sizeof(uint16_t));
                        if (tab_reg == NULL) {
                            free(tab_reg);
                            return MALLOC_REG_NULL;
                        }
                        memset(tab_reg, 0, cmdPara.number * sizeof(uint16_t));
                        cmdPara.pMapping= tab_reg;  
                    }
                    break;
                default:
                    break;
            }

            cmdPara.statepub= nh_->create_publisher<agv_msgs::msg::MBSlaveStates>(name + "/state", 10);
            RCLCPP_INFO(rclcpp::get_logger("Modbus"), "[statepub] topic[%s] create success", name.c_str());
            
            switch (cmdPara.function) {
                case MODBUS_FC_READ_COILS:
                case MODBUS_FC_READ_DISCRETE_INPUTS:
                case MODBUS_FC_READ_HOLDING_REGISTERS:
                case MODBUS_FC_READ_INPUT_REGISTERS:
                    break;
                case MODBUS_FC_WRITE_SINGLE_COIL: {
                    cmdPara.wCFG_bit = std::make_shared<writeCFG<std_msgs::msg::UInt8MultiArray>>(name);
                    auto sub = nh_->create_subscription<std_msgs::msg::UInt8MultiArray>(
                        slave.deviceName + "/" + cmdPara.topic_name_sub,
                        rclcpp::QoS(10),
                        [this, cmdPara](const std_msgs::msg::UInt8MultiArray::SharedPtr msg) {
                            cmdPara.wCFG_bit->writeValue_cb(*msg);
                        });
                    cmdPara.sub = sub;
                    break;
                }
                case MODBUS_FC_WRITE_SINGLE_REGISTER: {
                    cmdPara.wCFG_reg = std::make_shared<writeCFG<std_msgs::msg::UInt16MultiArray>>(name);
                    auto sub = nh_->create_subscription<std_msgs::msg::UInt16MultiArray>(
                        slave.deviceName + "/" + cmdPara.topic_name_sub,
                        rclcpp::QoS(10),
                        [this, cmdPara](const std_msgs::msg::UInt16MultiArray::SharedPtr msg) {
                            cmdPara.wCFG_reg->writeValue_cb(*msg);
                        });
                    break;
                }
                case MODBUS_FC_WRITE_MULTIPLE_COILS: {
                    cmdPara.wCFG_bit = std::make_shared<writeCFG<std_msgs::msg::UInt8MultiArray>>(name);
                    auto sub = nh_->create_subscription<std_msgs::msg::UInt8MultiArray>(
                        slave.deviceName + "/" + cmdPara.topic_name_sub,
                        rclcpp::QoS(10),
                        [this, cmdPara](const std_msgs::msg::UInt8MultiArray::SharedPtr msg) {
                            cmdPara.wCFG_bit->writeValue_cb(*msg);
                        });
                    cmdPara.sub = sub;
                    break;
                }
                case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: {
                    cmdPara.wCFG_reg = std::make_shared<writeCFG<std_msgs::msg::UInt16MultiArray>>(name);
                    auto sub = nh_->create_subscription<std_msgs::msg::UInt16MultiArray>(
                        slave.deviceName + "/" + cmdPara.topic_name_sub,
                        rclcpp::QoS(10),
                        [this, cmdPara](const std_msgs::msg::UInt16MultiArray::SharedPtr msg) {
                            cmdPara.wCFG_reg->writeValue_cb(*msg);
                        });
                    cmdPara.sub = sub;
                    break;
                }
                case MODBUS_FC_WRITE_AND_READ_REGISTERS: {
                    cmdPara.wCFG_reg = std::make_shared<writeCFG<std_msgs::msg::UInt16MultiArray>>(name);
                    auto sub = nh_->create_subscription<std_msgs::msg::UInt16MultiArray>(
                        slave.deviceName + "/" + cmdPara.topic_name_sub,
                        rclcpp::QoS(10),
                        [this, cmdPara](const std_msgs::msg::UInt16MultiArray::SharedPtr msg) {
                            cmdPara.wCFG_reg->writeValue_cb(*msg);
                        });
                    cmdPara.sub = sub;
                    break;
                }
                default:
                    RCLCPP_ERROR(rclcpp::get_logger("Modbus"), "Unknown modbus function: 0x%02X", cmdPara.function);
                    break;
            }
            slave.readParas.push_back(cmdPara);
        }
        m_Slaves.push_back(slave);
    }

    m_init_ok = true;

    return connect_host();
}

int ModbusSlave::init(std::string host_ip,int host_port,int slave_id)
{

    return connect_host();
}


void ModbusSlave::thread_cycle(ModbusSlave *pBase)
{
    if (!pBase)
    {
        RCLCPP_ERROR(rclcpp::get_logger("Modbus"), "modbus client thread pBase can't be null!!"); 
        return;
    }

    if (!pBase->m_init_ok)
    {
        RCLCPP_ERROR(rclcpp::get_logger("Modbus"), "modbus client thread must be inited before start!!"); 
        return;
    }
    if (m_ctx == nullptr){
        RCLCPP_INFO(rclcpp::get_logger("Modbus"), "polling cts is nullptr");
        return ;
    }
    std::chrono::system_clock::time_point begin_ = NOW_TIME;
    std::chrono::system_clock::time_point end_ = NOW_TIME;
    std::chrono::system_clock::time_point last_ = NOW_TIME;
    double pollelapsed_secs_ = 0.0;
    double interval_secs_ = 0.0;
    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "ModbusSlave polling thread"); 
    while (!pBase->m_quit_mutex.try_lock_for(std::chrono::milliseconds(m_threadInterval)))
    {
        begin_ = NOW_TIME;
        if(!polling()){
            if (m_reconnect) 
                reconnect_host();
        }  

        end_ = NOW_TIME;
        pollelapsed_secs_ = std::chrono::duration_cast<std::chrono::duration<double>>((end_ - begin_)).count();
        if (pollelapsed_secs_ < m_pollInterval)
        {
            RCLCPP_WARN(rclcpp::get_logger("Modbus"), "Control cycle is taking to fast, elapsed: %f, m_Slaves size:%lu",pollelapsed_secs_, m_Slaves.size());
            rclcpp::sleep_for(200ms); 
        }
        interval_secs_  = std::chrono::duration_cast<std::chrono::duration<double>>((end_ - last_)).count();
        last_= end_;
        RCLCPP_INFO(rclcpp::get_logger("Modbus"), "[thread_cycle]whole polling is taking: %fsec,from pre thread to now taking:%fsec",pollelapsed_secs_,interval_secs_);
        threadStatePub(pollelapsed_secs_, interval_secs_);
    }
}

void ModbusSlave::threadStatePub(float pollelapsed_secs, float interval_secs)
{
    agv_msgs::msg::MBCmdState  cmdState;
    cmdState.cmd_elapsed_secs = pollelapsed_secs;
    cmdState.cmd_whole_secs = interval_secs;
    agv_msgs::msg::MBSlaveStates states;
    states.mbcmdstates.push_back( cmdState);
    threadStatePub_->publish(states); 
}

//judge modbus polling
bool ModbusSlave::polling()
{
    bool rc;
    std::lock_guard<std::mutex> locker(m_mutex);
    std::chrono::system_clock::time_point temp_begin = NOW_TIME;
    std::vector<Slave_Para>::iterator itSlave = m_Slaves.begin();
    for (; itSlave != m_Slaves.end(); itSlave++) {
        agv_msgs::msg::MBSlaveStates states;
        std::vector<MB_cyclePara>::iterator itCmd = itSlave->readParas.begin();
        for(; itCmd != itSlave->readParas.end(); itCmd++){         
            // need write sync check 
            // if ( (itCmd->wCFG_reg != NULL && itCmd->wCFG_reg->wCFG == itCmd->wCFG_reg->wCFG_last) 
            //   || (itCmd->wCFG_bit != NULL && itCmd->wCFG_bit->wCFG == itCmd->wCFG_bit->wCFG_last))
            //       continue;

            if (itCmd->wCFG_bit != NULL) 
            {
                // if ( itCmd->wCFG_bit->wCFG == itCmd->wCFG_bit->wCFG_last)
                // {
                //     continue;
                // }
                // else
                // {
                    copy2mapping((uint8_t *)itCmd->pMapping, itCmd->number, itCmd->wCFG_bit->wCFG);   
                // }
            }
            else if (itCmd->wCFG_reg != NULL) 
            {
                // if ( itCmd->wCFG_reg->wCFG == itCmd->wCFG_reg->wCFG_last)
                // {
                //     continue;
                // }
                // else
                // {
                    copy2mapping((uint16_t *)itCmd->pMapping, itCmd->number, itCmd->wCFG_reg->wCFG);   
                // }
            }

            // cycle sync check
            if ( itCmd->tickCount < itCmd->tickexpire ){
                itCmd->tickCount++;   
                continue;
            }
          
            itCmd->tickCount= 0;
            temp_begin = NOW_TIME;
            int ret = Execute_Cmd(*itCmd);

            if(ret > 0) 
            {
                if (itCmd->wCFG_reg != NULL)
                {
                    itCmd->wCFG_reg->wCFG_last == itCmd->wCFG_reg->wCFG;
                }
                else if (itCmd->wCFG_bit != NULL)
                {
                    itCmd->wCFG_bit->wCFG_last == itCmd->wCFG_bit->wCFG;
                }
            }

            itCmd->end_ = NOW_TIME;

            agv_msgs::msg::MBCmdState cmdState;
            cmdState.name= itCmd->topic_name;
            cmdState.cmd_elapsed_secs = std::chrono::duration_cast<std::chrono::duration<float>>((itCmd->end_ - temp_begin)).count();
            rclcpp::sleep_for(std::chrono::milliseconds(static_cast<long>(m_pollInterval * 1000)));
            if (ret == -1)
            {
                RCLCPP_ERROR(rclcpp::get_logger("Modbus"), "[failed]:[%s-%s]",itSlave->deviceName.c_str(),itCmd->topic_name.c_str());
                fprintf(stderr, "polling slave failed : %s\n", modbus_strerror(errno));
                rc = false;
                continue;
            }
            rc = true;
      
            itCmd->end_ = NOW_TIME;
            cmdState.cmd_whole_secs = std::chrono::duration_cast<std::chrono::duration<float>>((itCmd->end_ - itCmd->begin_)).count();
            states.mbcmdstates.push_back(cmdState);
            itCmd->begin_ = NOW_TIME;
            itCmd->statepub->publish(states);
        }
        
    }
    
    return rc;
}

void ModbusSlave::pubMsg(MB_cyclePara& cmdPara)
{

    switch(cmdPara.arr_type) { 
        case COIL_STATUS:    //  0x：可读可写的PLC线圈个数    
        case INPUT_STATUS:      //1x： 只读PLC输入点     
            {
                std_msgs::msg::UInt8MultiArray bit_buffer;
                int bytesLen= cmdPara.number / Bits_PER_BYTE;
                for(int i= 0; i < bytesLen; i++)
                {
                    uint8_t ret= modbus_get_byte_from_bits( (uint8_t *)cmdPara.pMapping, i * Bits_PER_BYTE, Bits_PER_BYTE);
                    bit_buffer.data.push_back( ret);
                }
                int rem= cmdPara.number % Bits_PER_BYTE;
                if ( rem != 0)
                {
                    uint8_t ret=  modbus_get_byte_from_bits( (uint8_t *)cmdPara.pMapping, bytesLen * Bits_PER_BYTE, rem);
                    bit_buffer.data.push_back(ret);
                }

                auto pub = std::dynamic_pointer_cast<
                    rclcpp::Publisher<std_msgs::msg::UInt8MultiArray, std::allocator<void>>
                    >(cmdPara.pub);

                if (pub) {
                    pub->publish(bit_buffer);
                } else {
                    RCLCPP_ERROR(rclcpp::get_logger("modbus"), "Type cast failed for UInt8MultiArray publisher");
                }    
            }
            break;
        case HOLDING_REGISTER:     //4x： 可读可写的 PLC数据寄存器
        case INPUT_REGISTER:     //3x：  只读的PLC寄存器
        {
            std_msgs::msg::UInt16MultiArray reg_buffer;
            reg_buffer.data.resize(cmdPara.number);
            for(int i=0; i < cmdPara.number; i++)
                reg_buffer.data[i] = ((uint16_t *)cmdPara.pMapping)[i];

            auto pub = std::dynamic_pointer_cast<
                rclcpp::Publisher<std_msgs::msg::UInt16MultiArray, std::allocator<void>>
            >(cmdPara.pub);
            if (pub) {
                pub->publish(reg_buffer);
            } else {
                RCLCPP_ERROR(rclcpp::get_logger("modbus"), "Type cast failed for UInt16MultiArray publisher");
            }    
        }
            break;
        default:
            break;
    }
}


int ModbusSlave::Execute_Cmd(MB_cyclePara& cmdPara)
{
    int rc;
    RCLCPP_INFO_STREAM(rclcpp::get_logger("Modbus"), "Execute_Cmd id:" << cmdPara.slaveID << ",debug:" << m_debug);
    /* Check the fuction code,execute modbus cmd */
    if (m_debug)
        RCLCPP_INFO(rclcpp::get_logger("Modbus"), "\n\nExecute_Cmd:slaveID[%d],fun[%d],address[%d],number[%d]",cmdPara.slaveID,cmdPara.function,cmdPara.address, cmdPara.number);

    if (modbus_set_slave(m_ctx, cmdPara.slaveID) < 0) 
        RCLCPP_ERROR_STREAM(rclcpp::get_logger("Modbus"), "error modbus set slave:" << cmdPara.slaveID);

    switch (cmdPara.function) {
        case MODBUS_FC_READ_COILS:
            rc = modbus_read_bits(m_ctx, cmdPara.address, cmdPara.number, (uint8_t *)cmdPara.pMapping);
            break;
        case MODBUS_FC_READ_DISCRETE_INPUTS:
            rc = modbus_read_input_bits(m_ctx, cmdPara.address, cmdPara.number, (uint8_t *)cmdPara.pMapping);
            break;      
        case MODBUS_FC_READ_HOLDING_REGISTERS:
            rc = modbus_read_registers(m_ctx, cmdPara.address, cmdPara.number, (uint16_t *)cmdPara.pMapping);  
            break; 
        case MODBUS_FC_READ_INPUT_REGISTERS:
            rc = modbus_read_input_registers(m_ctx, cmdPara.address, cmdPara.number, (uint16_t *)cmdPara.pMapping);
            break; 
        case MODBUS_FC_WRITE_SINGLE_COIL:
            {
                rc = modbus_write_bit(m_ctx, cmdPara.address, *(uint8_t *)cmdPara.pMapping);     
                break;    
            }     
        case MODBUS_FC_WRITE_SINGLE_REGISTER:
            {
                rc = modbus_write_register(m_ctx, cmdPara.address, *(uint16_t *)cmdPara.pMapping);    
                break;  
            }       
        case MODBUS_FC_WRITE_MULTIPLE_COILS:
            {
                rc = modbus_write_bits(m_ctx, cmdPara.address, cmdPara.number, (uint8_t *)cmdPara.pMapping);   
                break; 
            }                         
        case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
            {
                rc = modbus_write_registers(m_ctx, cmdPara.address, cmdPara.number, (uint16_t *)cmdPara.pMapping);                
                break; 
            }     
        case MODBUS_FC_WRITE_AND_READ_REGISTERS:
            {
                rc = modbus_write_and_read_registers(m_ctx, 
                                                cmdPara.address, cmdPara.number, 
                                                (uint16_t *)cmdPara.pMapping,
                                                cmdPara.address, cmdPara.number, 
                                                (uint16_t *)cmdPara.pMapping);                       
                break;      
            }
        default:
            fprintf(stderr, "unknown modbus function\n");
            rc=  -1;
            break;
    }
    modbus_flush(m_ctx);
    if(rc >= 0) 
        pubMsg(cmdPara);        
    else
        fprintf(stderr, "%s\n", modbus_strerror(errno));  
    return rc;
}


bool ModbusSlave::reconnect_host()
{
  std::lock_guard<std::mutex> locker(m_mutex);
    if (m_ctx)
    {
        modbus_close(m_ctx);     
        if (modbus_connect(m_ctx) != -1)
        {
            modbus_set_response_timeout(m_ctx, 0, m_resptime);
            std::cout << "reconnect success\n\n\n";
            return true;
        }
    }
    fprintf(stderr, "modbus_tcp_io connection failed:%s\n", modbus_strerror(errno));
    return false;
}

int ModbusSlave::connect_host()
{
   std::lock_guard<std::mutex> locker(m_mutex);

    if (m_use_backend == TCP) {
        m_ctx = modbus_new_tcp(m_host_ip.data(), m_host_port);
    } else if (m_use_backend == TCP_PI) {
        m_ctx = modbus_new_tcp_pi(m_host_ip.data(), "1502");
    } else {
        m_ctx = modbus_new_rtu(m_uart.port.data(), m_uart.baudrate, *m_uart.patity.data(), m_uart.data_bits, m_uart.stop_bit);
    }
    modbus_set_debug(m_ctx, m_debug);
    modbus_set_response_timeout(m_ctx, 0, m_resptime); 
    if (m_ctx == nullptr)
    {
        fprintf(stderr, "Unable to allocate libmodbus contex\n");
        return CTX_NULLPTR;
    }
    if (modbus_connect(m_ctx) == -1)   //等待连接设备
    {
        fprintf(stderr, "connection failed:%s\n", modbus_strerror(errno));
        return FAILED_CONNECT;
    }
    
    std::cout << "modbus connect success";
    return 0;
}
